/* 
 * File:   CCalibration.cpp
 * Author: andrea
 * 
 * Created on November 11, 2013, 4:34 PM
 */

#include "../Headers/includes.h"
#include "CCalibration.h"

using namespace std;
using namespace cv;

CCalibration::CCalibration() {
}

/**
 * Searches inside the directory for jpg images and finds the corners. Returns a vector of vectors
 * of points. Each element of the outer vector, is the vector of points referred to image coordinates.
 * @param path string directory path
 * @return vector<vector<cv::Point3f>> the points in default coordinates
 */
vector < vector < Point2f > > CCalibration::findCornersFromDir(string path, Size boardSize) {
    CUtilities util;
    vector<string> files = util.getFilesListFromDir(path);
    vector < vector < Point2f > > retValue;
    int i = 0;
    for (vector<string>::iterator it = files.begin(); it != files.end(); ++it) {
        cout << "Extracting corners from:" << *it << endl;

        vector<Point2f> tmpPointBuf;

        Mat tmpImage = imread(*it, 0);
        bool cornersFound = findChessboardCorners(tmpImage, boardSize, tmpPointBuf, CV_CALIB_CB_ADAPTIVE_THRESH);
        if (cornersFound) {
            retValue.push_back(tmpPointBuf);
            namedWindow(*it);
            i++;
            if (i == 400) {
                Mat tmpCorners(tmpImage.rows, tmpImage.cols, CV_8UC3);
                drawChessboardCorners(tmpCorners, boardSize, tmpPointBuf, cornersFound);
                imshow(*it, tmpCorners);
                waitKey(0);
            }
        }
    }
    return retValue;
}

void CCalibration::calibrate(vector< vector < Point2f > > imagePoints, Mat cameraMatrix, vector<Mat> rotVecs, vector<Mat> transVecs, Mat distCoeffs, Size boardSize, Size imageSize) {
    //  Generate the reference system points
    vector < vector<Point3f> > refSysPoints;
    for (int k = 0; k < imagePoints.size(); k++) {
        vector<Point3f> objectPointsSingle;
        for (int i = 0; i < boardSize.width; i++) {
            for (int j = 0; j < boardSize.height; j++) {
                Point3f tmpPoint(i, j, 0);
                objectPointsSingle.push_back(tmpPoint);
            }
        }
        refSysPoints.push_back(objectPointsSingle);
    }

    calibrateCamera(refSysPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rotVecs, transVecs);
}
